#ifndef MSG_FMT_H
#define	MSG_FMT_H
#include <string.h>
#include <arpa/inet.h>
#include <endian.h>
#include <assert.h>
#include "pkcs11.h"

/*
 *	len(2) | result(4) | [reserved] | data.... 
 */
#define	MSG_RESERVED	(24 - 6)
#define	MSG_HEADER	(24)

static inline void MSG_ENCODE_8(uint8_t v, uint8_t **p)
{
	*(*p) = v;
	(*p)++;
}

static inline void MSG_DECODE_8(uint8_t **p, uint8_t *dst)
{
	(*dst) = *(*p);
	(*p)++;
}

static inline void MSG_ENCODE_16(uint16_t v, uint8_t **p)
{
	v = htons(v);
	(*(uint16_t *)(*p)) = v;
	(*p) += 2;
}

static inline void MSG_DECODE_16(uint8_t **p, uint16_t *dst)
{
	uint16_t v = *(uint16_t *)(*p);

	v = ntohs(v);
	(*dst) = v;
	(*p) += 2;
}

static inline void MSG_ENCODE_32(uint32_t v, uint8_t **p)
{
	v = htonl(v);
	(*(uint32_t *)(*p)) = v;
	(*p) += 4;
}

static inline void MSG_DECODE_32(uint8_t **p, uint32_t *dst)
{
	uint32_t v = *(uint32_t *)(*p);

	v = ntohl(v);
	(*dst) = v;
	(*p) += 4;
}

static inline void CK_USER_TYPE_pack(CK_USER_TYPE v, uint8_t **p)
{
	uint8_t n = v;
	MSG_ENCODE_8(n, p);
}

static inline void CK_USER_TYPE_unpack(uint8_t **p, CK_USER_TYPE *v)
{
	uint8_t n;
	MSG_DECODE_8(p, &n);
	(*v) = (CK_USER_TYPE)n;
}

static inline void __DATA_pack(uint8_t *data, uint16_t len, uint8_t **p)
{
	uint8_t *cur = *p;
	uint16_t lenlen = len;

	lenlen = htons(lenlen);
	*(uint16_t *)cur = lenlen;
	cur += 2;

	if(len > 0) {
		memcpy(cur, data, len);
		cur += len;
	}
	(*p) = cur;
}

static inline void DATA_pack(CK_BYTE_PTR data, CK_ULONG len, uint8_t **p)
{
	uint8_t *cur = *p;
	uint16_t lenlen = (uint16_t)len;

	lenlen = htons(lenlen);
	*(uint16_t *)cur = lenlen;
	cur += 2;

	if(len > 0) {
		memcpy(cur, data, len);
		cur += len;
	}
	(*p) = cur;
}

static inline void DATA_pack_no_copy(CK_BYTE_PTR data, CK_ULONG len, uint8_t **p)
{
	uint16_t lenlen = (uint16_t)len;

	lenlen = htons(lenlen);
	*(uint16_t *)(*p) = lenlen;

	(*p) += 2;
	if(len > 0)
	{
		(*p) += len;
	}
}

static inline void __DATA_unpack(uint8_t **p, uint8_t **dst, uint16_t *dstlen) 
{
	uint8_t	*cur = *p;
	uint16_t lenlen;

	lenlen = *(uint16_t *)cur;
	lenlen = ntohs(lenlen);
	cur += 2;

	if(lenlen > 0) {
		(*dst) = cur;
		cur += lenlen;
	}
	(*dstlen) = lenlen;
	(*p) = cur;
}

static inline void DATA_unpack_copy(uint8_t **p, CK_BYTE_PTR dst, CK_ULONG *dstlen) 
{
	uint8_t	*cur = *p;
	uint16_t lenlen;

	lenlen = *(uint16_t *)cur;
	lenlen = ntohs(lenlen);
	cur += 2;

	if(lenlen > 0) {
		if(dst != NULL) {
			memcpy(dst, cur, lenlen);
		}
		cur += lenlen;
	}
	if(dstlen != NULL) {
		(*dstlen) = (CK_ULONG)lenlen;
	}
	(*p) = cur;
}

static inline void DATA_unpack_nocopy(uint8_t **p, CK_BYTE_PTR *dst, CK_ULONG *dstlen) 
{
	uint8_t	*cur = *p;
	uint16_t lenlen;

	lenlen = *(uint16_t *)cur;
	lenlen = ntohs(lenlen);
	cur += 2;

	if(lenlen > 0) {
		*dst = cur;
		cur += lenlen;
	}
	(*dstlen) = (CK_ULONG)lenlen;
	(*p) = cur;
}

static inline void DATA_unpack2(uint8_t **p, CK_BYTE_PTR dst, CK_ULONG dstlen) 
{
	CK_ULONG n;

	DATA_unpack_copy(p, dst, &n);
	assert(n == dstlen);
}

static inline void MSG_ENCODE_INIT(uint8_t *buffer, uint8_t **p, uint32_t cmd_result) 
{
	uint8_t	*cur = buffer;

	/* lenlen */
	cur += 2; 

	/* cmd or result */
	cmd_result = htonl(cmd_result); 
	*(uint32_t *)cur = cmd_result; 
	cur += 4;

	/* reserved */
	cur += MSG_RESERVED; 
	(*p) = cur;
}

static inline void MSG_DECODE_INIT(uint8_t *buffer, uint8_t **p, uint32_t *cmd_result)
{
	uint32_t v;
	uint8_t *cur = buffer;

	cur += 2 ;
	if(cmd_result)
	{
		v = *(uint32_t *)cur;
		(*cmd_result) = ntohl(v);
	}
	cur += 4;
	cur += MSG_RESERVED;
	(*p) = cur;
}

static inline void  CK_MECHANISM_pack(CK_MECHANISM_PTR m, uint8_t **p)
{
	uint16_t n = m->mechanism;
	MSG_ENCODE_16(n, p); 
	__DATA_pack((uint8_t *)m->pParameter, (uint16_t)m->ulParameterLen, p);
}

static inline void CK_MECHANISM_unpack(uint8_t **p, CK_MECHANISM_PTR m)
{
	uint8_t *dst;
	uint16_t n;
	memset(m, 0, sizeof(CK_MECHANISM));
	MSG_DECODE_16(p, &n); 
	m->mechanism = (CK_MECHANISM_TYPE)n;
	__DATA_unpack(p, &dst, &n); 
	m->pParameter = dst;
	m->ulParameterLen = (CK_ULONG)n;
}

static inline void CK_ATTRIBUTE_pack(CK_ATTRIBUTE_PTR attr, uint8_t **p)
{
	MSG_ENCODE_16((uint16_t)attr->type, p);
	__DATA_pack((uint8_t *)attr->pValue, (uint16_t)attr->ulValueLen, p);
}

static inline void CK_ATTRIBUTE_unpack(uint8_t **p, CK_ATTRIBUTE_PTR attr)
{
	uint8_t *dst;
	uint16_t n;
	memset(attr, 0, sizeof(CK_ATTRIBUTE));
	MSG_DECODE_16(p, &n);
	attr->type = (CK_ATTRIBUTE_TYPE)n;
	__DATA_unpack(p, &dst, &n);                      
	attr->pValue = dst;
	attr->ulValueLen = (CK_ULONG)n;
}

static inline void CK_OBJECT_HANDLE_pack(CK_OBJECT_HANDLE obj, uint8_t **p)
{
	memcpy(*p, &obj, sizeof(CK_OBJECT_HANDLE));
	(*p) += sizeof(CK_OBJECT_HANDLE);
}

static inline void CK_OBJECT_HANDLE_unpack(uint8_t **p, CK_OBJECT_HANDLE_PTR obj) 
{
	memcpy(obj, *p, sizeof(CK_OBJECT_HANDLE));
	(*p) += sizeof(CK_OBJECT_HANDLE);
}

static inline void CK_ULONG_pack(CK_ULONG v, uint8_t **p)
{
	MSG_ENCODE_16((uint16_t)v, p);
}

static inline void CK_ULONG_unpack(uint8_t **p, CK_ULONG *v)
{
	uint16_t n;
	MSG_DECODE_16(p, &n);
	(*v) = (CK_ULONG)n;
}

static inline void CK_ATTRIBUTE_ARRAY_pack(CK_ATTRIBUTE_PTR attr, CK_ULONG num, uint8_t **p)
{
	CK_ULONG i;
	uint16_t n = (uint16_t)num;
	CK_ATTRIBUTE_PTR one;

	MSG_ENCODE_16(n, p);
	for(i = 0; i < num; i++)
	{
		one = attr + i;
		CK_ATTRIBUTE_pack(one, p);
	}
}

/*
 *	copy to attr
 */
static inline void CK_ATTRIBUTE_ARRAY_unpack(uint8_t **p, CK_ATTRIBUTE_PTR attr, CK_ULONG num)
{
	int	i;
	uint16_t n;
	CK_ATTRIBUTE_PTR one;

	MSG_DECODE_16(p, &n);
	assert(n == (uint16_t)num);

	for(i = 0; i < num; i++)
	{
		one = attr + i;
		MSG_DECODE_16(p, &n);
		one->type = (CK_ATTRIBUTE_TYPE)n;
		if(n > 0)
		{
			DATA_unpack_copy(p, one->pValue, &one->ulValueLen); 
		}
	}
}

static inline void ARRAY_SIZE_pack(CK_ULONG v, uint8_t **p)
{
	MSG_ENCODE_16((uint16_t)v, p);
}

static inline void ARRAY_SIZE_unpack(uint8_t **p, CK_ULONG *v)
{
	uint16_t n;
	MSG_DECODE_16(p, &n);
	(*v) = (CK_ULONG)n;
}

static inline void CK_ULONG_ARRAY_pack(CK_ULONG_PTR v, CK_ULONG num, uint8_t **p)
{
	int	i;
	CK_ULONG_PTR one;

	MSG_ENCODE_16((uint16_t)num, p);
	for(i = 0; i < num; i++)
	{
		one = v + i;
		memcpy(*p, one, sizeof(CK_ULONG)); 
		(*p) += sizeof(CK_ULONG);
	}
}

static inline void CK_ULONG_ARRAY_unpack(uint8_t **p, CK_ULONG_PTR v, CK_ULONG_PTR num)
{
	int	i;
	uint16_t n;
	CK_ULONG_PTR one;
	MSG_DECODE_16(p, &n);
	(*num) = (CK_ULONG)n;

	for(i = 0; i < n; i++)
	{
		one = v + i;
		memcpy(one, *p, sizeof(CK_ULONG));
		(*p) += sizeof(CK_ULONG);
	}
}

static inline void CK_OBJECT_HANDLE_ARRAY_pack(CK_OBJECT_HANDLE_PTR obj, CK_ULONG num, uint8_t **p)
{
	int	i;
	uint16_t n = (uint16_t)num;

	MSG_ENCODE_16(n, p);
	for(i = 0; i < num; i++)
	{
		CK_OBJECT_HANDLE_PTR one = obj + i;
		memcpy(*p, one, sizeof(CK_OBJECT_HANDLE));
		(*p) += sizeof(CK_OBJECT_HANDLE);
	}
}

static inline void CK_OBJECT_HANDLE_ARRAY_unpack(uint8_t **p, CK_OBJECT_HANDLE_PTR obj, CK_ULONG_PTR num)
{
	int i;
	uint16_t n;
	CK_OBJECT_HANDLE_PTR one;

	MSG_DECODE_16(p, &n);
	(*num) = (CK_ULONG)n;

	for(i = 0; i < n; i++)
	{
		one = obj + i;
		memcpy(one, *p, sizeof(CK_OBJECT_HANDLE));
		(*p) += sizeof(CK_OBJECT_HANDLE);
	}
}

static inline int MSG_GET_CMD(uint8_t *p)
{
	uint32_t cmd;

	cmd = *(uint32_t *)(p+2);
	cmd = ntohl(cmd);
	return cmd;
}

#endif

